using System.Collections.Generic;
using Unity.MLAgents;
using UnityEngine;
using MBaske.Sensors.Grid;
using MBaske.MLUtil;

namespace MBaske.Dogfight
{
    public class DogEnvController : MonoBehaviour
    {
        [System.Serializable]
        public class PlayerInfo
        {
            public PilotAgent Agent;
            [HideInInspector]
            public Vector3 StartingPos;
            [HideInInspector]
            public Quaternion StartingRot;
            //[HideInInspector]
            //public Rigidbody Rb;
        }


        /// <summary>
        /// Max Academy steps before this platform resets
        /// </summary>
        /// <returns></returns>
        [Tooltip("Max Environment Steps")] public int MaxEnvironmentSteps = 25000;

        /// <summary>
        /// The area bounds.
        /// </summary>

        /// <summary>
        /// We will be changing the ground material based on success/failue
        /// </summary>


        //List of Agents On Platform
        public List<PlayerInfo> AgentsList = new List<PlayerInfo>();

        private Dogsetting m_dogsetting;


        private SimpleMultiAgentGroup m_BlueAgentGroup;
        private SimpleMultiAgentGroup m_RedAgentGroup;

        private int m_ResetTimer;

        void Start()
        {

            m_dogsetting = FindObjectOfType<Dogsetting>();
            // Initialize TeamManager
            m_BlueAgentGroup = new SimpleMultiAgentGroup();
            m_RedAgentGroup = new SimpleMultiAgentGroup();
        
            
            foreach (var item in AgentsList)
            {
                item.StartingPos = item.Agent.transform.position;
                item.StartingRot = item.Agent.transform.rotation;
                //item.Rb = item.Agent.GetComponent<Rigidbody>();
                
                
                //Debug.Log(item.Agent.team);

                if (item.Agent.team == Team.Blue)
                {
                    m_BlueAgentGroup.RegisterAgent(item.Agent);
                }
                else
                {
                    m_RedAgentGroup.RegisterAgent(item.Agent);
                }

                /*
                Debug.Log(item.Agent.team);

                if (item.Agent.team == Team.Blue)
                {
                    m_BlueAgentGroup.RegisterAgent(item.Agent);
                }
                else
                {
                    m_RedAgentGroup.RegisterAgent(item.Agent);
                }
                */
                
                // 
                /*
                Debug.Log(item.Agent.m_BehaviorParameters.TeamId);

                if (item.Agent.m_BehaviorParameters.TeamId == (int)Team.Blue)
                {
                    m_BlueAgentGroup.RegisterAgent(item.Agent);
                }
                else
                {
                    m_RedAgentGroup.RegisterAgent(item.Agent);
                }
                */

            }
            
            // 打印每个组的成员
            /*  
            foreach(var m_agent in m_RedAgentGroup.GetRegisteredAgents())
            {   
                Debug.Log(m_agent); 
            } 
            foreach(var m_agent in m_BlueAgentGroup.GetRegisteredAgents())
            {   
                Debug.Log(m_agent); 
            } 
            */
            ResetScene();
        }

        void FixedUpdate()
        {
            m_ResetTimer += 1;
/*
            // 不在if内部声明是为了扩大作用域限制
            var agentred1=0;
            var agentred2=0;
            var agentblue1=0;
            var agentblue2=0;

            //Debug.Log("debug in DogEnvController.cs"); 
            int i=0;
            foreach (var item in AgentsList)
            {
                if (i == 0)
                {
                    agentred1 = item;
                }
                else if (i == 1)
                {
                    agentred2 = item;

                }
                 else if (i == 2)
                {
                    agentblue1 = item;

                }
                 else
                {
                    agentblue2 = item;

                }
                i++; 
                   Debug.Log(agentblue2.Agent.team);
               
            }
*/
            var agentred1 = AgentsList[0];
            var agentred2 = AgentsList[1];
            var agentblue1 = AgentsList[2];
            var agentblue2 = AgentsList[3];

            Vector3 vlcagentred1 =agentred1.Agent.m_Ship.WorldVelocity;
            Vector3 vlcagentred2 =agentred2.Agent.m_Ship.WorldVelocity;
            Vector3 vlcagentblue1=agentblue1.Agent.m_Ship.WorldVelocity;
            Vector3 vlcagentblue2=agentblue2.Agent.m_Ship.WorldVelocity;
            Vector3 posagentred1 = agentred1.Agent.m_Ship.transform.position;
            Vector3 posagentred2 = agentred2.Agent.m_Ship.transform.position;
            Vector3 posagentblue1 = agentblue1.Agent.m_Ship.transform.position;
            Vector3 posagentblue2 = agentblue2.Agent.m_Ship.transform.position;
            
            //red  group
            Vector3 deltar1b1 = posagentblue1-posagentred1;
            Vector3 deltar1b2 = posagentblue2-posagentred1;
            float speedr1b1 = Vector3.Dot(deltar1b1.normalized, vlcagentred1);
            float speedr1b2 = Vector3.Dot(deltar1b2.normalized, vlcagentred1);
            
            m_RedAgentGroup.AddGroupReward(speedr1b1* 0.01f+speedr1b2* 0.01f);

            if (speedr1b1 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_BlueAgentGroup.AddGroupReward(speedr1b1 * -0.005f);
            }
            if (speedr1b2 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_BlueAgentGroup.AddGroupReward(speedr1b2 * -0.005f);
            }

            Vector3 deltar2b1 = posagentblue1-posagentred2;
            Vector3 deltar2b2 = posagentblue2-posagentred2;
            float speedr2b1 = Vector3.Dot(deltar2b1.normalized, vlcagentred2);
            float speedr2b2 = Vector3.Dot(deltar2b2.normalized, vlcagentred2);
            
            m_RedAgentGroup.AddGroupReward(speedr2b1* 0.01f+speedr2b2* 0.01f);

            if (speedr2b1 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_BlueAgentGroup.AddGroupReward(speedr2b1 * -0.005f);
            }
            if (speedr2b2 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_BlueAgentGroup.AddGroupReward(speedr2b2 * -0.005f);
            }

             //blue  group
            Vector3 deltab1r1 = posagentred1-posagentblue1;
            Vector3 deltab1r2 = posagentred2-posagentblue1;
            float speedb1r1 = Vector3.Dot(deltab1r1.normalized, vlcagentblue1);
            float speedb1r2 = Vector3.Dot(deltab1r2.normalized, vlcagentblue1);
            
            m_BlueAgentGroup.AddGroupReward(speedb1r1* 0.01f+speedb1r2* 0.01f);

            if (speedb1r1 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_RedAgentGroup.AddGroupReward(speedb1r1 * -0.005f);
            }
            if (speedb1r2 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_RedAgentGroup.AddGroupReward(speedb1r2 * -0.005f);
            }

            Vector3 deltab2r1 = posagentred1-posagentblue2;
            Vector3 deltab2r2 = posagentred2-posagentblue2;
            float speedb2r1 = Vector3.Dot(deltab2r1.normalized, vlcagentblue2);
            float speedb2r2 = Vector3.Dot(deltab2r2.normalized, vlcagentblue2);
            
            m_BlueAgentGroup.AddGroupReward(speedb2r1 * 0.01f+speedb2r2 * 0.01f);

            if (speedb2r1 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_RedAgentGroup.AddGroupReward(speedb2r1 * -0.005f);
            }
            if (speedb2r2 > 0)
            {
                // Penalize opponent for being followed. speed * -0.005f
                m_RedAgentGroup.AddGroupReward(speedb2r2 * -0.005f);
            }


            if (m_ResetTimer >= MaxEnvironmentSteps && MaxEnvironmentSteps > 0)
            {
                m_BlueAgentGroup.GroupEpisodeInterrupted();
                m_RedAgentGroup.GroupEpisodeInterrupted();
                ResetScene();
            }
        }
        
        // 引导飞机打飞机
        public void Goaltrend(Team scoredTeam)
        {
            if (scoredTeam == Team.Blue)
            {
                m_BlueAgentGroup.AddGroupReward(1 - (float)m_ResetTimer / MaxEnvironmentSteps);
                m_RedAgentGroup.AddGroupReward(-1);
            }
            else
            {
                m_RedAgentGroup.AddGroupReward(1 - (float)m_ResetTimer / MaxEnvironmentSteps);
                m_BlueAgentGroup.AddGroupReward(-1);
            }                

        }
        
        public void GoalTouched(Team scoredTeam)
        {
            if (scoredTeam == Team.Blue)
            {
                m_BlueAgentGroup.AddGroupReward(1 - (float)m_ResetTimer / MaxEnvironmentSteps);
                m_RedAgentGroup.AddGroupReward(-1);
            }
            else
            {
                m_RedAgentGroup.AddGroupReward(1 - (float)m_ResetTimer / MaxEnvironmentSteps);
                m_BlueAgentGroup.AddGroupReward(-1);
            }
            m_BlueAgentGroup.EndGroupEpisode();
            m_RedAgentGroup.EndGroupEpisode();
            
            
            Debug.Log(scoredTeam);
            
            
            ResetScene();

        }


        public void ResetScene()
        {
            m_ResetTimer = 0;

            //Reset Agents
           /*
            foreach (var item in AgentsList)
            {
                var randomPosX = Random.Range(-5f, 5f);
                var newStartPos = item.Agent.initialPos + new Vector3(randomPosX, 0f, 0f);
                var rot = item.Agent.rotSign * Random.Range(80.0f, 100.0f);
                var newRot = Quaternion.Euler(0, rot, 0);
                item.Agent.transform.SetPositionAndRotation(newStartPos, newRot);

                //item.Rb.velocity = Vector3.zero;
               //item.Rb.angularVelocity = Vector3.zero;

            }
            */

        }
    }
}